function [pose_error, map_error, mapdata] = fastslam3d(landmark, action)

%--------------------------------------------------------------------------
% Simluator for 6DOF Fastslam2 adapted from T. Bailey's MATLAB code
%--------------------------------------------------------------------------
%
%  function mapOutput = sim1(landmark, action)
%
%  Input
%   landmark - set of 3D landmarks
%   action - action sequences
%  Output
%   mapOutput - map that contains position of observed landmarks
% 
%  Note: An external file 'dataset.mat' contains a test set of 3D landmarks 
%        and action sequences. It has to be loaded before calling this
%        function.
%  -----
%  - robot is a simple model and it contains no geometry. It is represented
%    by 6 parameters (x,y,z,roll,pitch,yaw)
%--------------------------------------------------------------------------

close all
configfile;

% testing: robot stays at origin and observe landmarks
%--------------------------------------------------------------------------
% Simluation stuffs
%  Initialize graphic
graphic = setup_graphic(landmark,action);
%  Initialize system
%  Init MCU pose
MCU_graphic_X = [0 0; -200 -200; -200 -200]; % define simple 2 triangle shape that represents the MCU
MCU_graphic_Y = [0 0; -100   0;  100   0]; 
MCU_graphic_Z = [0 0;  0    0;   0  100];

MCU_graphic_X_est = [0 0; -200 -200; -200 -200]; % define simple 2 triangle shape that represents the MCU
MCU_graphic_Y_est = [0 0; -100   0;  100   0]; 
MCU_graphic_Z_est = [0 0;  0    0;   0  100];

% initialize particles
particles = initialise_particles(NR_PARTICLES);

% true pose of the MCU
xtrue= zeros(6,1);
xtrue(3) = 0;
xtrue_predict = zeros(6,1);
xtrue_predict(3) = 0;
tr_param = zeros(6,1);
ntr_param = zeros(6,1);

index_current_action= 1; % index to first action 
steering_angle = [0; 0; 0]; % initial steer angle is zero
direction = [0;0;0]; % unit vector that represents the current direction of movement
predict_times = 0;
control_noise_times = 0;
ftag= 1:size(landmark,2); % identifier for each landmark
da_table= zeros(1,size(landmark,2)); % data association table 
zs = [];
this_iteration = 0;
pose_error = [];
map_error = [];
xPv = [];
%--------------------------------------------------------------------------

%    waitforbuttonpress

% loop
while index_current_action ~= 0 
    
    % update action
    [tr_param ,index_current_action]= update_action(xtrue, action, index_current_action,...
                                                              tr_param, MAX_T, MAX_R);

    % update index of current waypoint
    if index_current_action == 0 & NUMBER_LOOPS > 1
        index_current_action=1; NUMBER_LOOPS= NUMBER_LOOPS-1; 
    end
    
    % calculate new MCU true pose
    xtrue = calculate_true_pose(xtrue, tr_param);

    % add control noise
    [ntr_param]= add_control_noise(tr_param,Q, SWITCH_CONTROL_NOISE);
    
    %======================================================================
    % FastSLAM
    % 1) PREDICT
    %  for every particle
    %  1.1) predict current pose according to input and add random noise to it
    %  1.2) predict current state covariance

    % predict true pose
    for i= 1:NR_PARTICLES
        particles(i) = predict(particles(i), ntr_param, Q);
%        particles(i).xv
%        particles(i).Pv
    end

   
    % 2) OBSERVE
    %  for every particle
    %  2.1) compute weight

    % Observe step
    predict_times = predict_times + 1;
    if predict_times >= OBSERVE_INTERVAL
        predict_times = 0;

        [z,ftag_visible]= get_observations(xtrue, landmark, ftag, MAX_RANGE);
        z= add_observation_noise(z,R, SWITCH_SENSOR_NOISE); % sim stuff

        % plotting stuff
        zs = [];
        for i = 1:size(ftag_visible,2)
            zs = [zs,landmark(:,ftag_visible(i))];        
        end
        
        % Compute (known) data associations
        Nf= size(particles(1).xf,2);
        [zf,idf,zn,da_table]= data_associate_known(z, ftag_visible, da_table, Nf);
        %   find associations (zf) and new features (zn)

       
        
        % Observe map features
        if ~isempty(zf) % if not empty
            % compute weights w = w * p(z_k | x_k-1)
            for i=1:NR_PARTICLES
                w= compute_weight(particles(i), zf,idf, R);

               particles(i).w= particles(i).w * w;
%                www = particles(i).w
            end
       
            
%            xxxPv = [particles.Pv]
            

            % resampling *before* computing proposal permits better particle diversity
            particles= resample_particles(particles, NEFFECTIVE, 1);            

    
            % sample from "optimal" proposal distribution, then update map 
            for i=1:NR_PARTICLES
                particles(i)= sample_proposal(particles(i), zf,idf, R, 1);  % Pv = 0

                
                particles(i)= feature_update(particles(i), zf, idf, R);

%                 disp('wait for button pressed')
%                 waitforbuttonpress

            end
        end

        % Observe new features, augment map
        if ~isempty(zn)
            for i=1:NR_PARTICLES
                if isempty(zf) % sample from proposal distribution (if we have not already done so above)
                    particles(i).xv= multivariate_gauss(particles(i).xv, particles(i).Pv, 1);
                    particles(i).Pv= zeros(6);
                end                        
                particles(i)= add_feature(particles(i), zn,R);
%                 disp('wait for button pressed')
%                 waitforbuttonpress

            end
        end

        
    end

              
            
    xvp = [particles.xv];

    xfp = [particles.xf];

  
    xxppff = [particles.Pf];
    
%    summ = sum(trace(xxppff(:)))
%    summ = sum(sum(xvp(:)))
    
    w = [particles.w];
%waitforbuttonpress 
    ii= find(w== max(w));
    xvmax= xvp(:,ii);

%     for i=1:NR_PARTICLES    
%         xPv = [xPv, particles(i).Pv];
%     end
%     
%     temp_error = 0;
%     if size(ii,2) == 1
%         xPvmax = xPv(ii);
%         temp_error = trace(xPvmax)
%     end
%     if size(ii,2) > 1 & size(xPv,2) > 1
%         temp_error = trace(xPv(1))
%     end
%     xPv = [];
                  

    
%     temp_error = [xtrue(1) - xvmax(1);
%                   xtrue(2) - xvmax(2);
%                   xtrue(3) - xvmax(3);
%                   xtrue(4) - xvmax(4);
%                   xtrue(5) - xvmax(5);
%                   xtrue(6) - xvmax(6)];

     temp_error = xtrue(4) - xvmax(4);

    pose_error = [pose_error, temp_error];

%    pose_error = [pose_error, summ];
%     
%     if ~isempty(xfp)
%         particles(1).xf;
%         %        xfpmax = xfp(:,ii).xf
%         landmark
%        map_error = [map_error, sum(landmark(:) - particles(1).xf(:))];
%     end
    

    
    mapdata = particles(ii);
%     xtrue
    
    % update graphic
     [xt_x, xt_y, xt_z] = transform_mcu_graphic(xtrue, MCU_graphic_X, MCU_graphic_Y, MCU_graphic_Z);
     set(graphic.MCU_xt1, 'xdat', xt_x(:,1),'ydat', xt_y(:,1),'zdat', xt_z(:,1))
     set(graphic.MCU_xt2, 'xdat', xt_x(:,2),'ydat', xt_y(:,2),'zdat', xt_z(:,2))

     [xtn_x, xtn_y, xtn_z] = transform_mcu_graphic(xvmax, MCU_graphic_X_est, MCU_graphic_Y_est, MCU_graphic_Z_est);
%     xtn_x = MCU_graphic_X_est;   xtn_y = MCU_graphic_Y_est;   xtn_z = MCU_graphic_Z_est;
     set(graphic.MCU_xt1_est, 'xdat', xtn_x(:,1),'ydat', xtn_y(:,1),'zdat', xtn_z(:,1))
     set(graphic.MCU_xt2_est, 'xdat', xtn_x(:,2),'ydat', xtn_y(:,2),'zdat', xtn_z(:,2))
% 
    if ~isempty(zs)
        set(graphic.seen_lm, 'xdat', zs(1,:), 'ydat', zs(2,:),'zdat', zs(3,:))
    end

    if ~isempty(xfp)
        set(graphic.est_lm, 'xdat', xfp(1,:), 'ydat', xfp(2,:),'zdat', xfp(3,:))
    end

     if ~isempty(xvp)
        set(graphic.est_pos, 'xdat', xvp(1,:), 'ydat', xvp(2,:),'zdat', xvp(3,:))
    end

    drawnow
end
    
                                                       
% FastSLAM
% 1) PREDICT
%  for every particle
%  1.1) predict current pose according to input and add random noise to it
%  1.2) predict current state covariance
% 2) OBSERVE
%  for every particle
%  2.1) compute weight
% 3) RESAMPLING
%  3.1) resample if weight variance is less than Nmin
%  3.2) sample proposal
% 4) UPDATE LANDMARKS
%  for every landmark
%  4.1) EKF
